import java.lang.Math;
import lejos.nxt.*;
import lejos.robotics.localization.*;
import lejos.robotics.navigation.*;

public class MapScanner implements Runnable {

  PoseProvider poseProvider;
  UltrasonicSensor usensor;
  float map[][];
  int width;
  boolean done;
  
  public MapScanner(PoseProvider poseProvider, UltrasonicSensor usensor, int width) {
    map = new float[width][width];
    this.poseProvider = poseProvider;
    this.usensor = usensor;
    this.width = width;
    for (int x = 0; x < width; ++x)
      for (int y = 0; y < width; ++y)
	map[x][y] = .5f;
    done = false;
  }
  
  public void run() {
    while(!done) {
      // get Position and distance:
      Pose pose = poseProvider.getPose();
      int x = (int) pose.getX()/2;
      int y = (int) (pose.getY()+64.f)/2;
      int dx = 0, dy = 0;
      int ox = 0, oy = 0;
      float heading = pose.getHeading();
      int distance = usensor.getDistance()/2;
      
      // TODO: Make generic using trigonometry 'n stuff
      try {
	if (Math.abs(heading) < 1.f || Math.abs(heading-360.f) < 1.f) {
	  //System.out.println("vorwaerts");
	  dy = 1;
	  for (int i = y; i < oy-2; ++i)
	    updateMap(ox, i, 0.2f);
	}
	else if (Math.abs(heading-90.f) < 1.f) {
	  //System.out.println("rechts");
	  dx = -1;
	  for (int i = x; i > ox+2; --i)
	    updateMap(i, oy, 0.2f);
	}
	else if (Math.abs(heading-180.f) < 1.f) {
	  //System.out.println("rueckwaerts");
	  dy = -1;
	  for (int i = y; i > oy+2; --i)
	    updateMap(ox, i, 0.2f);
	}
	else if (Math.abs(heading+90.f) < 1.f) {
	  //System.out.println("links");
	  dx = 1;
	  for (int i = x; i < ox-2; ++i)
	    updateMap(i, oy, 0.2f);
	}
	if (dy != 0 || dx != 0) {
	  ox = x + distance * dx;
	  oy = y + distance * dy;
	  //System.out.println("set: " + ox + "/" + oy);
	  updateMap(ox, oy, 0.8f);
	  updateMap(ox+1*dx, oy+1*dy, 0.6f);
	  updateMap(ox-1*dx, oy-1*dy, 0.6f);
	  updateMap(ox+2*dx, oy+2*dy, 0.5f);
	  updateMap(ox-2*dx, oy-2*dy, 0.5f);
	}
      }
      catch (ArrayIndexOutOfBoundsException e) {
	//System.out.println("OOB: " + ox + "/" + oy);
      }
    }
  }
  
  public void updateMap(int x, int y, float measurement) {
    map[x][y] = 1.f/(1.f+((1.f-measurement)*(1.f-map[x][y]))/(measurement*map[x][y]));
  }
}